#include "Arduino.h"
#include "inmp441.h"

void i2s_init() {
  i2s_config_t i2s_config = {
    .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX),
    .sample_rate = I2S_SAMPLE_RATE,
    .bits_per_sample = I2S_BITS_PER_SAMPLE_32BIT,
    .channel_format = I2S_CHANNEL,
    .communication_format =  (i2s_comm_format_t)(I2S_COMM_FORMAT_I2S | I2S_COMM_FORMAT_I2S_MSB),
    .intr_alloc_flags = ESP_INTR_FLAG_LEVEL1,
    .dma_buf_count = 8,
    .dma_buf_len = 256,
    .use_apll = false,
    .tx_desc_auto_clear = true,
    .fixed_mclk = 0
  };

  i2s_pin_config_t pin_config = {
    .bck_io_num = PIN_I2S_BCLK,
    .ws_io_num = PIN_I2S_LRC,
    .data_out_num = I2S_PIN_NO_CHANGE,
    .data_in_num = PIN_I2S_DIN
  };

  i2s_driver_install(I2S_PORT, &i2s_config, 0, NULL);
  i2s_set_pin(I2S_PORT, &pin_config);
  i2s_set_clk(I2S_PORT, I2S_SAMPLE_RATE, I2S_BITS_PER_SAMPLE_32BIT, I2S_CHANNEL_MONO);
  i2s_zero_dma_buffer(I2S_PORT);
}

void inmp441_init(){
    i2s_init();
}

size_t inmp441_listen(int16_t* buff, size_t buff_size_bytes) {
  size_t samples_out = buff_size_bytes / sizeof(int16_t);
  if (samples_out == 0) {
    return 0;
  }

  size_t bytes_to_read = samples_out * sizeof(int32_t);

  static int32_t i2s_temp[2048];
  if (samples_out > (sizeof(i2s_temp) / sizeof(i2s_temp[0]))) {
    samples_out = sizeof(i2s_temp) / sizeof(i2s_temp[0]);
    bytes_to_read = samples_out * sizeof(int32_t);
  }

  size_t bytes_read = 0;
  i2s_read(I2S_PORT, (void*)i2s_temp, bytes_to_read, &bytes_read, portMAX_DELAY);

  size_t samples_read = bytes_read / sizeof(int32_t);

  static int32_t hpf_y_prev = 0;
  static int32_t hpf_x_prev = 0;
  const int32_t hpf_a = 32700;

  for (size_t i = 0; i < samples_read; i++) {
    int32_t s = i2s_temp[i];
    int32_t s24 = s >> 8;
    int32_t y = (s24 - hpf_x_prev) + (int32_t)(((int64_t)hpf_a * hpf_y_prev) / 32768);
    hpf_x_prev = s24;
    hpf_y_prev = y;
    int32_t s16 = y >> 8;
    if (s16 > 32767) s16 = 32767;
    if (s16 < -32768) s16 = -32768;
    buff[i] = (int16_t)s16;
  }

  return samples_read * sizeof(int16_t);
}